{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Tutorial 02: commonroad-io Interface\n",
    "\n",
    "The collision checker library provides a convenient interface to *commonroad-io*. In order to illustrate the functionality, we load a CommonRoad scenario.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import os\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "from commonroad.common.file_reader import CommonRoadFileReader\n",
    "from commonroad_cc.visualization.draw_dispatch import draw_object\n",
    "\n",
    "# load the exemplary CommonRoad scenario using the CommonRoad file reader\n",
    "scenario, planning_problem_set = CommonRoadFileReader('ZAM_Tutorial-1_2_T-1.xml').open()\n",
    "\n",
    "# plot the scenario\n",
    "plt.figure(figsize=(25, 10))\n",
    "draw_object(scenario)\n",
    "draw_object(planning_problem_set)\n",
    "plt.autoscale()\n",
    "plt.gca().set_aspect('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 1. Converting CommonRoad Objects to Collision Objects\n",
    "\n",
    "All shapes and obstacles can be converted to collision objects in order to perform intersection tests. Therefore, we need to call the function 'create_collision_object()'. The supported CommonRoad objects are:\n",
    "\n",
    "    - commonroad.geometry.shape.ShapeGroup\n",
    "    - commonroad.geometry.shape.Polygon\n",
    "    - commonroad.geometry.shape.Circle\n",
    "    - commonroad.geometry.shape.Rectangle\n",
    "    - commonroad.scenario.obstacle.StaticObstacle\n",
    "    - commonroad.scenario.obstacle.DynamicObstacle\n",
    "    - commonroad.prediction.prediction.SetBasedPrediction\n",
    "    - commonroad.prediction.prediction.TrajectoryPrediction\n",
    "    \n",
    "Note that the trajectories of dynamic obstacles are not interpolated. So collisions are only considered at discrete points in time."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_object\n",
    "\n",
    "plt.figure(figsize=(25, 10))\n",
    "\n",
    "draw_object(scenario.lanelet_network)\n",
    "\n",
    "# convert each static obstacle in the scenario to a collision object and plot it\n",
    "for obs in scenario.static_obstacles:\n",
    "    draw_object(create_collision_object(obs), draw_params={'collision': {'facecolor': 'red'}})\n",
    "\n",
    "# convert each dynamic obstacle in the scenario to a collision object and plot it\n",
    "for obs in scenario.dynamic_obstacles:\n",
    "    draw_object(create_collision_object(obs), draw_params={'collision': {'facecolor': 'blue'}})\n",
    "\n",
    "plt.autoscale()\n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Creating a Collision Checker from the Scenario\n",
    "\n",
    "A pycrcc.CollisionChecker object can be directly generated from a CommonRoad scenario:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_checker\n",
    "\n",
    "cc = create_collision_checker(scenario)\n",
    "\n",
    "plt.figure(figsize=(25, 10))\n",
    "draw_object(scenario.lanelet_network)\n",
    "draw_object(cc, draw_params={'collision': {'facecolor': 'blue'}})\n",
    "plt.autoscale()\n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Given the collision checker for the scenario, it can be easily checked if a trajectory of a ego vehicle collides with an object in the environment."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from commonroad.scenario.trajectory import State, Trajectory\n",
    "from commonroad.prediction.prediction import TrajectoryPrediction\n",
    "from commonroad.geometry.shape import Rectangle\n",
    "\n",
    "# create a trajectory for the ego vehicle starting at time step 0\n",
    "position = np.array([[2.5, 0.0], [4.5, 0.0], [6.5, 0.0], [8.5, 0.0], [10.5, 0.0], [12.5, 0.0], [14.5, 0.0]])\n",
    "state_list = list()\n",
    "for k in range(0, len(position)):\n",
    "    state_list.append(State(**{'position': position[k], 'orientation': 0.0}))\n",
    "trajectory = Trajectory(0, state_list)\n",
    "\n",
    "# create the shape of the ego vehicle\n",
    "shape = Rectangle(length=4.5, width=2.0)\n",
    "# create a TrajectoryPrediction object consisting of the trajectory and the shape of the ego vehicle\n",
    "traj_pred = TrajectoryPrediction(trajectory=trajectory, shape=shape)\n",
    "\n",
    "# create a collision object using the trajectory prediction of the ego vehicle\n",
    "co = create_collision_object(traj_pred)\n",
    "\n",
    "# test the trajectory of the ego vehicle for collisions\n",
    "print('Collision between the trajectory of the ego vehicle and objects in the environment: ', cc.collide(co))\n",
    "\n",
    "plt.figure(figsize=(25, 10))\n",
    "draw_object(scenario.lanelet_network)\n",
    "draw_object(cc, draw_params={'collision': {'facecolor': 'blue'}})\n",
    "draw_object(co, draw_params={'collision': {'facecolor': 'green'}})\n",
    "plt.autoscale()\n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3. Minkowski Sum \n",
    "\n",
    "The *commonroad-io* interface offers the possibility to compute the minkowski sum with a circle and an arbitrary *commonroad-io* shape before adding the shape to the collision checker."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from commonroad_cc.collision_detection.pycrcc_collision_dispatch import create_collision_checker\n",
    "\n",
    "cc = create_collision_checker(scenario, params={'minkowski_sum_circle': True, \n",
    "                                                'minkowski_sum_circle_radius': 2.0,\n",
    "                                                'resolution': 4})\n",
    "\n",
    "plt.figure(figsize=(25, 10))\n",
    "draw_object(scenario.lanelet_network)\n",
    "draw_object(cc, draw_params={'collision': {'facecolor': 'blue'}})\n",
    "plt.autoscale()\n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
